#pragma once

#include "stdafx.h"
#include "tnt.h"

#define TIME_STEP .012f
#define FRICTION 0.0f
#define POSITION 0.050f
#define VELOCITY 0.0025f
#define ACCELERATION 0.005f

/*#define TIME_STEP .015f
#define FRICTION 0.0f
#define POSITION 1.0f
#define VELOCITY 1.0f
#define ACCELERATION 15.0f*/

class KalmanTank
{
public:
	KalmanTank(otank_t* enemy);
	~KalmanTank(void);

	void UpdateMatricies();
	Vector2 Predict(float deltaTime);
	Vector2 GetCurrentEstimate();
	float Sigma_X();
	float Sigma_Y();
	float Rho();
	std::string color();

	void ResetEx();

private:
	TNT::Array2D<float> CalcCommon();

	TNT::Array2D<float> M;
	TNT::Array2D<float> M1;
	TNT::Array2D<float> K;
	TNT::Array2D<float> Ex;
	TNT::Array2D<float> Ez;
	TNT::Array2D<float> Et;
	TNT::Array2D<float> H;
	TNT::Array2D<float> F;
	TNT::Array2D<float> Ht;
	TNT::Array2D<float> Ft;
	TNT::Array2D<float> I;

	otank_t* enemy;
};

